#include "imgprovider.h"
#include <algorithm>
#include <functional>

ImgProvider::ImgProvider()
{
    /// 存储数据队列的方法
//    std::lock_guard<mutex> lg(g_mtx);
//    auto d = std::make_tuple(1,2,3);
//    dequeData.push_back(d);
//    g_CV.notify_all();
    _mkdir("grid/real");
    initBackThread();

    deqT = new DequeThread;
}


void ImgProvider::initBackThread()
{
    /// 添加常驻线程用于存储图像及算法结果
    std::thread *back = new std::thread([this](){
        unique_lock<mutex> ul(g_mtx);
        sleep_s(5);
        while (getSystemStatus()) {
            while (!dequeData.empty()) {
                std::ofstream ofs("grid/real/pos.txt",ios_base::app);
                auto data = dequeData.front();
                // 调用日志系统函数处理  图像，数据，时间等信息
                cv::Mat mat = std::get<0>(data);
                auto pos =  std::get<1>(data);
                auto uv =  std::get<2>(data);
                cv::imwrite(pos.toStr(),mat);
                ofs <<  std::get<3>(data);  // 插入时间
                ofs << pos.toStr() << "\t" << std::to_string(uv.u) << "\t"  << std::to_string(uv.v);
                ofs << std::endl;
                dequeData.pop_front();
                ofs.close();
            }
            sleep_s(2);
        }
        qWarning()<<"[ImgProvider::initBackThread]图像队列存储系统关闭!"<<__FUNCTION__;
    });

    ThreadPool::GetInstance()->pushPermanentAsync(MU_THREAD_SAVE,back);

}




// 格点拍照,去一个点停一下然后拍照
void ImgProvider::callImagePoss(string creatName, GenPosVector &poss, bool correct)
{
    if(isRun.load()){
        return;
    }
    std::ofstream ofs;
    try {
        isRun.store(true);
        std::atomic<bool> tkEnd = ATOMIC_FLAG_INIT;     // 图像采集的周期
        std::deque<Gen_Run>  rundata;
        std::vector<GenPos> _list;    // 当前需要运动的一些点

        for(GenPos &posYZ  : std::get<2>(poss)){  // 获取格点矩阵pos
            // auto pos = std::get<0>(posYZ);
            _list.push_back(posYZ);
        }

        std::string date = "grid/" + creatName.substr(0,10) + "/";  // 获取时间
        _mkdir(date);
        string savePath = "grid/" + creatName + "/";
        _mkdir(savePath);

        ofs.open(savePath+"pos.txt");

        /* 存储偏移值 */
        {
            std::ofstream rfs(savePath+"details.txt");
            if(std::get<3>(poss)){
                rfs << "当前为数据库点\r\n";
                rfs << std::get<4>(poss).db_name<< "\t";
                rfs << std::get<4>(poss).seamid  << "\t";
                rfs << std::get<4>(poss).index1;
                rfs << "\r\n";
            }
            rfs << "原始相机中心点:";
            rfs << std::get<1>(poss).toStr();       // 原始点
            rfs << "\r\n";
            rfs << "旋转矩阵:";
            rfs << std::get<5>(poss).toStr() ;       // 旋转矩阵
            rfs << endl;

            rfs << "变位机参数:";
            rfs << std::get<0>(poss).toStr() ;       // 变位机参数
            rfs << endl;


            rfs.close();
        }

        // RobotHelper::GetInstance()->MoveToSafePos();

        if(std::get<3>(poss)){      // 如果是数据库中的扫描点
            PLCHelper::GetInstance()->CloseAirValve();       /* 回收传感器汽阀 */
            MachineHelper::GetInstance()->ABSMove(std::get<0>(poss),true);  /* 移动到安全位姿 */
            MachineHelper::GetInstance()->ABSMove(std::get<0>(poss));  /* 变位机旋转到指定位置 */
        }

        LaserHelper::GetInstance()->Open();

        // RobotHelper::GetInstance()->ProcessContinousPos(conPos);

        // 图形获取线程
        auto result = std::async(std::launch::async,[this,&tkEnd,&savePath,&rundata,&_list](){

                static int index = 0;
                for(auto it:_list){
                    auto p = std::get<0>(it);    // 获取龙骨的base-pos
                    RobotHelper::GetInstance()->ABSMove(p);
                    auto mat = CameraHelper::GetInstance()->GetPictrue();
                    rundata.push_back(std::make_tuple(mat,it,index,p.toStr()));
                    index++;
                    /* 此处做进度管理 */
                    PMsg temp;
                    temp.curP = index;
                    temp.totalP = _list.size();
                    temp.curMsg = p.toStr();
                    reflushMsg(temp);
                }
                tkEnd.store(true);
                index = 0;
        });


        // 图形存储线程
        auto save = std::async(std::launch::async,[&tkEnd,&savePath,&rundata,&ofs](){

            while(!tkEnd.load()){
                while(!rundata.empty()){
                    auto mat = std::get<0>(rundata.front());
                    auto gp = std::get<1>(rundata.front());
                    //auto posStr = std::get<0>(gp);
                    auto yz = std::get<1>(gp);
                    int index = std::get<2>(rundata.front());
                    string f = savePath + std::to_string(index) + ".png";
                    auto posStr = std::get<3>(rundata.front());
                    cv::imwrite(f,mat);
                    ofs<<std::to_string(index) << "\t" <<yz.ypp << "\t" <<yz.zpp <<"\t" << posStr  ;
                    ofs << endl;
                    rundata.pop_front();
                }
            }
        });
        result.get();
        save.get();
    } catch(MU_EXPS::CAMERA_EXPECTION e){
        /// 写入日志
        qWarning()<< BOLDRED<<__FUNCTION__<<"相机采集数据失败!"<< RESET ;
        pushWarn(MU_Messages::Camera,e,MU_ERROR_MODULE_CAM);
    }catch(MU_EXPS::ROBOT_EXPECTION e){
        /// 写入日志
        qWarning()<< BOLDRED<<"Robot异常!"<<__FUNCTION__<< RESET ;
        pushWarn(MU_Messages::Robot,e,MU_ERROR_MODULE_CAM);
    }catch(std::system_error e){
        std::cout << BOLDRED << e.what() << RESET <<std::endl;
    }
    catch (...) {
        ofs.close();
        /// 写入日志
        qWarning()<< BOLDRED<<"图像算法调用失败!"<<__FUNCTION__<< RESET ;
        // throw MU_EXPS::EXP_IMAGE_IMAGE_FAIL;
        pushWarn(MU_Messages::Image,MU_EXPS::EXP_IMAGE_IMAGE_FAIL,MU_ERROR_MODULE_IMAGE);
    }
    isRun.store(false);
}

void ImgProvider::callImageConPoss(string creatName, GenPosVector &poss)
{
    std::ofstream ofs;
    try {
        std::vector<GenPos> _list;    // 当前需要运动的一些点
        std::vector<RobotPos> conPos;   //
        std::atomic<bool> tkEnd = ATOMIC_FLAG_INIT;
        std::deque<Gen_Run>  rundata;

        for(GenPos &posYZ  : std::get<2>(poss)){  // 获取格点矩阵pos
            auto p = std::get<0>(posYZ);    // 获取龙骨的base-pos
            conPos.push_back(p);
            _list.push_back(posYZ);
        }
        string savePath = "grid/" + creatName + "/";
        _mkdir(savePath);

        ofs.open(savePath+"pos.txt");
        /* 存储偏移值 */
        {
            std::ofstream rfs(savePath+"details.txt");
            if(std::get<3>(poss)){
                rfs << "当前为数据库点\r\n";
                rfs << std::get<4>(poss).db_name<< "\t";
                rfs << std::get<4>(poss).seamid  << "\t";
                rfs <<std::get<4>(poss).index1;
                rfs << "\r\n";
            }
            rfs << "原始相机中心点:";
            rfs << std::get<1>(poss).toStr();       // 原始点
            rfs << "\r\n";
            rfs << "旋转矩阵:";
            rfs << std::get<5>(poss).toStr() ;       // 旋转矩阵
            rfs << endl;

            rfs << "变位机参数:";
            rfs << std::get<0>(poss).toStr() ;       // 变位机参数
            rfs << endl;

            rfs.close();
        }

        // RobotHelper::GetInstance()->MoveToSafePos();

        if(std::get<3>(poss)){      // 如果是数据库中的扫描点
            PLCHelper::GetInstance()->CloseAirValve();       /* 回收传感器汽阀 */
            MachineHelper::GetInstance()->ABSMove(std::get<0>(poss),true);  /* 移动到安全位姿 */

            MachineHelper::GetInstance()->ABSMove(std::get<0>(poss));  /* 变位机旋转到指定位置 */
        }

        LaserHelper::GetInstance()->Open();
        RobotHelper::GetInstance()->ProcessContinousPos(conPos);
        // 图形获取线程
        auto result = std::async(std::launch::async,[&rundata,&_list](){
            while (!RobotHelper::GetInstance()->GetConEnd()) {
                auto mat = CameraHelper::GetInstance()->GetPictrue();
                auto curPos = RobotHelper::GetInstance()->getCurrentPos();
                for(auto it:_list){  // 实时获取当前pos，对照容器，如果有，则存入数据
                    if(std::get<0>(it) == curPos){
                        static int index = 0;
                        rundata.push_back(std::make_tuple(mat,it,index,curPos.toStr()));
                         index++;
                    }
                }
            }
        });


        result.get();

        // 图形存储线程
        auto save = std::async(std::launch::async,[&tkEnd,&savePath,&rundata,&ofs](){

            while(!tkEnd.load()){
                while(!rundata.empty()){
                    auto mat = std::get<0>(rundata.front());
                    auto gp = std::get<1>(rundata.front());
                    //auto posStr = std::get<0>(gp);
                    auto yz = std::get<1>(gp);
                    int index = std::get<2>(rundata.front());
                    string f = savePath + std::to_string(index) + ".png";
                    auto posStr = std::get<3>(rundata.front());
                    cv::imwrite(f,mat);
                    ofs<<std::to_string(index) << "\t" <<yz.ypp << "\t" <<yz.zpp <<"\t" << posStr  ;
                    ofs << endl;
                    rundata.pop_front();
                }
            }
        });
        save.get();
    } catch(MU_EXPS::CAMERA_EXPECTION e){
        /// 写入日志
        qWarning()<< BOLDRED<<"相机采集数据失败!"<<__FUNCTION__<< RESET ;
        pushWarn(MU_Messages::Camera,e,MU_ERROR_MODULE_CAM);
    }catch(MU_EXPS::ROBOT_EXPECTION e){
        /// 写入日志
        qWarning()<< BOLDRED<<"Robot异常!"<<__FUNCTION__<< RESET ;
        pushWarn(MU_Messages::Robot,e,MU_ERROR_MODULE_CAM);
    }catch(std::system_error e){
        std::cout << BOLDRED << e.what() << RESET <<std::endl;
    }
    catch (...) {
        ofs.close();
        /// 写入日志
        qWarning()<< BOLDRED<<"图像算法调用失败!"<<__FUNCTION__<< RESET ;
        throw MU_EXPS::EXP_IMAGE_IMAGE_FAIL;
        pushWarn(MU_Messages::Image,MU_EXPS::EXP_IMAGE_IMAGE_FAIL,MU_ERROR_MODULE_IMAGE);
    }
}

std::pair<RobotPos,RobotPos> ImgProvider::findRealPos(GenPosVector &poss)
{
    unique_lock<mutex> ul(g_mtx);
    try {
        // 运动思想：创建一个map
        std::map<std::string,cv::Mat> _map;
        std::vector<RobotPos> _list;    // 格点容器
        for(GenPos &posYZ  : std::get<2>(poss)){  // 获取格点矩阵pos
            auto p = std::get<0>(posYZ);
            _list.push_back(p);
        }

        PLCHelper::GetInstance()->CloseAirValve();       /* 回收传感器汽阀 */
        MachineHelper::GetInstance()->ABSMove(std::get<0>(poss),true);  /* 移动到安全位姿 */
        LaserHelper::GetInstance()->Open();
        RobotHelper::GetInstance()->MoveToSafePos();
        MachineHelper::GetInstance()->ABSMove(std::get<0>(poss));  /* 变位机旋转到指定位置 */

        bool conStop = false;   // 机器人连续运动的停止标志
        auto fr = RobotHelper::GetInstance()->ABSMove(_list);       // 开启连续运动模式
        // 图像获取线程
        auto result = std::async(std::launch::deferred,[this,&_list,&poss](bool stop) mutable{
            unique_lock<mutex> ul(g_mtx);
            RobotPos ret;
            // std::ofstream ofs("grid/real/pos.txt");
            while(!stop){
                auto mat = CameraHelper::GetInstance()->GetPictrue();
                auto curPos = RobotHelper::GetInstance()->getCurrentPos();
                _gxA->AddImg(mat);
                cv::Point2d p2d = _gxA->getuv();
                UV u1v = {p2d.x,p2d.y};
                RobotPos pi;
                UV2Point3D(curPos,u1v,pi,0);
                _list.push_back(pi);
                dequeData.push_back(std::make_tuple(mat,curPos,u1v,DateTime::getClock()));
                sleep_ms(50);
            }


            RobotPos realPos = RobotPos::instance();
            RobotPos mis;
            if(getCenter(_list,realPos)){
                auto p0 = std::get<1>(poss);
                ret = realPos>RobotTCP::CAMERA;
                mis = (!p0)<<ret;
            }
            return std::make_pair(ret,mis);
        },conStop);
        if( MU_RETURN_ERROR ==  fr.get()){
            qWarning()<< BOLDRED<<__FUNCTION__<<"多点运动没有执行!"<< RESET ;
        }
        conStop = true;
        return result.get();
    }catch(MU_EXPS::CAMERA_EXPECTION e){
        /// 写入日志
        qWarning()<< BOLDRED<<"相机采集数据失败!"<<__FUNCTION__<< RESET ;
        pushWarn(MU_Messages::Camera,e,MU_ERROR_MODULE_CAM);
    }catch(MU_EXPS::ROBOT_EXPECTION e){
        /// 写入日志
        qWarning()<< BOLDRED<<"Robot异常!"<<__FUNCTION__<< RESET ;
        pushWarn(MU_Messages::Robot,e,MU_ERROR_MODULE_CAM);
    }catch(std::system_error e){
        std::cout << BOLDRED << e.what() << RESET <<std::endl;
    }
    catch (...) {
        /// 写入日志
        qWarning()<< BOLDRED<<"图像算法调用失败!"<<__FUNCTION__<< RESET ;
        throw MU_EXPS::EXP_IMAGE_IMAGE_FAIL;
        pushWarn(MU_Messages::Image,MU_EXPS::EXP_IMAGE_IMAGE_FAIL,MU_ERROR_MODULE_IMAGE);
    }
    RobotPos realPos  = RobotPos::instance();
    return std::make_pair(realPos,realPos);
}
